package kit.edu.lego.gruppe3.levels.rotateBridge;

import kit.edu.lego.gruppe3.ComponentsRepository;
import kit.edu.lego.gruppe3.levels.LevelSelector;
import kit.edu.lego.gruppe3.levels.LevelSolver;
import kit.edu.lego.gruppe3.sensors.Scanner;
import lejos.robotics.navigation.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;

public class MoveBetweenLines implements Behavior{

	private DifferentialPilot robot = ComponentsRepository.getInstance().getRobotPilot();

	@Override
	public void action() {
		int leftAngle = Scanner.getFirstLine(true);
		int rightAngle = Scanner.getLastLine();
		
		int angleToRotate = (leftAngle - rightAngle) / 2;
		
		robot.setTravelSpeed(5);
		
		robot.rotate(angleToRotate);
		
		robot.forward();
	}

	@Override
	public void suppress() {
	}

	@Override
	public boolean takeControl() {
		return LevelSelector.getInstance().isRotatingBridge();
	}

}
